//
// Created by lewis on 2023/12/20.
//
#include <zmq.hpp>
#include <cstring>
#include <iostream>
#include <map>
#include <regex>

typedef std::map<std::string, double> KeyValueMap;

struct TargetInfo {
    uint8_t index;
    uint64_t time_stamp_ms;
    double lat;
    double lon;
    float alt;
    float spd_n;
    float spd_e;
    float spd_d;
};

int string_to_map(const std::string &data, KeyValueMap &kv_map);

int main()
{
    printf("build_branch:\t%s\nbuild_sha1:\t%s\nbuild_time:\t%s %s\n",
           GIT_BRANCH_NAME, GIT_COMMIT_SHA1, __DATE__, __TIME__);
           
    std::string render_ip = "172.25.176.1";
    int render_port = 14552;

    void* context_ = zmq_ctx_new();
    void* socket_ = zmq_socket(context_, ZMQ_REQ);

    std::string send_str = "tcp://" + render_ip + ":" + std::to_string(render_port);
    zmq_connect(socket_, send_str.c_str());

    //有效输入例如 : "Target", "tar", "Vehicle", "veh", "Obstacle", "obs"
    std::string request_str = "Obstacle";
    zmq_msg_t request_msg;
    zmq_msg_init_size(&request_msg, request_str.size());
    memcpy(zmq_msg_data(&request_msg), request_str.c_str(), request_str.size());
    zmq_msg_send(&request_msg, socket_, 0);
    zmq_msg_close(&request_msg);

    zmq_msg_t reply_message;
    zmq_msg_init(&reply_message);
    zmq_msg_recv(&reply_message, socket_, 0);

    std::string msg_str(static_cast<char*>(zmq_msg_data(&reply_message)), zmq_msg_size(&reply_message));
    int num =  std::stoi(msg_str);
    std::cout << "recv Obstacle num: " << num << std::endl;

    //有效输入例如："Target/1", "tar/1", "Vehicle/1", "veh/1", "Obstacle/1", "obs/1"
    request_str = "obs/1";
    zmq_msg_init_size(&request_msg, request_str.size());
    memcpy(zmq_msg_data(&request_msg), request_str.c_str(), request_str.size());
    zmq_msg_send(&request_msg, socket_, 0);
    zmq_msg_close(&request_msg);

    zmq_msg_init(&reply_message);
    zmq_msg_recv(&reply_message, socket_, 0);
    msg_str.clear();
    msg_str.append(static_cast<char*>(zmq_msg_data(&reply_message)), zmq_msg_size(&reply_message));

    if (zmq_msg_size(&reply_message) > 0)
    {
        KeyValueMap kv_map;
        string_to_map(msg_str, kv_map);
        TargetInfo target{0};
        target.index = (uint8_t)kv_map["id"];
        target.time_stamp_ms = (uint64_t)kv_map["time_stamp_ms"];
        target.lat = kv_map["lat"] * 1e-7;
        target.lon = kv_map["lon"] * 1e-7;
        target.alt = (float)kv_map["alt"];
        target.spd_n = (float)kv_map["vx"];
        target.spd_e = (float)kv_map["vy"];
        target.spd_d = (float)kv_map["vz"];
        std::cout << (int)target.index << ", " << target.lat << ", " << target.lon << ", " << target.spd_n << std::endl;
    }
    
    std::cout << "hello, get obstacle info" << std::endl;
    return 0;
}

int string_to_map(const std::string &data, KeyValueMap &kv_map)
 {
    std::regex pattern("Key:(\\w+)_Value:(-*\\d+\\.?\\d*);");
    std::smatch match;

    // Find and store key-value pairs
    std::string::const_iterator searchStart(data.cbegin());
    while (std::regex_search(searchStart, data.cend(), match, pattern)) {
        if (match.size() == 3) {
            double value = std::stod(match[2].str());
            kv_map[match[1].str()] = value;
        }
        searchStart = match.suffix().first;
    }

    // Check for any missing or invalid key-value pairs
    if (kv_map.empty()) {
        std::cout << "No valid key-value pairs found." << std::endl;
        return 0;
    }
    return 1;
}